package roborl.littleLegRobot;

import java.util.HashMap;
import java.util.Vector;

import roborl.Action;
import roborl.Decision;
import roborl.DiscreteValueApr;
import roborl.PhysRobot;
import roborl.PhysicsWorld;
import roborl.RobotState;
import roborl.StaticState;

import net.phys2d.math.Vector2f;
import net.phys2d.raw.ActuatorJoint;
import net.phys2d.raw.AngleJoint;
import net.phys2d.raw.Body;
import net.phys2d.raw.FixedMotorJoint;
import net.phys2d.raw.MotorJoint;
import net.phys2d.raw.shapes.Box;
import net.phys2d.raw.test.AngularConstraint;

public class ChainRobo extends PhysRobot {
	public static final Action rot_cw = new Action("ROT_CW");
	public static final Action rot_ccw = new Action("ROT_CCW");
	public static final Vector<Action> as;
	static {
		as = new Vector<Action>();
		as.add(rot_cw);
		as.add(rot_ccw);
	}
	// MotorJoint actuatorJoint;
	FixedMotorJoint fixedMotorJoint;
	Body bigBlock;
	Body stick;

	public ChainRobo() {
		super.DECISION_ITERATION = 50;
		super.SILENT_ITERATION = 25;
		DiscreteValueApr apr = new DiscreteValueApr(ChainRobo.as);
		Decision dec = new Decision(apr, new ChainReward());
		setDecision(dec);
		Vector<Object> parts = getParts();
		float x = 400;
		float y = 350;

		bigBlock = new Body(new Box(50, 20), 10);
		bigBlock.setFriction(1.2f);
		bigBlock.setPosition(x + 25, y - 10);
		parts.add(bigBlock);

		stick = new Body(new Box(20, 5), 1);
		stick.setPosition(x - 10, y - 2.5f);
		stick.setFriction(0.7f);
		stick.setMass(1);
		parts.add(stick);

		// ActuatorJoint actuatorJoint = new ActuatorJoint(stick,bigBlock,new
		// Vector2f(-20,2.5f),new Vector2f(25,7.5f));
		// actuatorJoint = new MotorJoint(stick, bigBlock, new Vector2f(7, 1f),
		// new Vector2f(-25, 3f));
		// parts.add(actuatorJoint);

		Vector<Float> angleSet = new Vector<Float>();
		for (int i = 0; i < 6; i++) {
			angleSet.add((float) (i / 6.0f * 2 * Math.PI));
		}
		fixedMotorJoint = new FixedMotorJoint(stick, bigBlock, new Vector2f(7,
				2.5f), new Vector2f(-25, 10f));
		fixedMotorJoint.setAngularSpeed(0.2f);
		parts.add(fixedMotorJoint);
		fixedMotorJoint.setAngleSet(angleSet);
		// AngleJoint angleJoint = new AngleJoint(stick, bigBlock, new
		// Vector2f(7,
		// 1f), new Vector2f(-25, 3f), 0, 1);
		// parts.add(angleJoint);

	}

	private final double mod(double a, double b) {
		int c = (int) (a / b);
		a -= c * b;
		if (a < 0)
			a += b;
		return a;
	}

	public boolean isIn(double a, double b, double c) {
		double ca = mod(c - a, 2 * Math.PI);
		double ba = mod(b - a, 2 * Math.PI);
		return ca < ba;
	}

	@Override
	public RobotState getState() {
		// TODO Auto-generated method stub
		// TODO Auto-generated method stub
		int subDivide = 6;
		HashMap<String, Double> s = new HashMap<String, Double>();
		s.put("speed", (double) (bigBlock.getVelocity().getX() < 0 ? 1 : -1));
		s.put("stickstate", (double) ((((int) (stick.getRotation()
				/ (2 * Math.PI) * subDivide))
				% subDivide + subDivide) % subDivide));

		Vector<Double> hs = new Vector<Double>();
		hs.add((double) bigBlock.getPosition().getX());
		return new StaticState(s, hs);
	}

	@Override
	protected void simpleUpdate() {
		// TODO Auto-generated method stub
		if (bigBlock.getPosition().getX() > 700) {
			lastAction = null;
			for (Object o : parts) {
				if (o instanceof Body) {
					((Body) o).adjustPosition(new Vector2f(-300, 00));
				}

			}
		} else if (bigBlock.getPosition().getX() < 100) {
			lastAction = null;
			for (Object o : parts) {
				if (o instanceof Body) {
					((Body) o).adjustPosition(new Vector2f(300, 00));
				}
			}
		}
	}

	@Override
	public void takeAction(Action action) {
		// TODO Auto-generated method stub
		// Vector<Object> parts = getParts();
		if (action == rot_cw) {
			// fixedMotorJoint.setAngularSpeed(0.5f);
			int ca = fixedMotorJoint.getCurrentAngleSet();
			ca++;
			if (ca > 5)
				ca = 0;
			fixedMotorJoint.setCurrentAngleSet(ca);
		} else if (action == rot_ccw) {
			// fixedMotorJoint.setAngularSpeed(-0.5f);
			int ca = fixedMotorJoint.getCurrentAngleSet();
			ca--;
			if (ca < 0)
				ca = 5;
			fixedMotorJoint.setCurrentAngleSet(ca);
		} else {
			// fixedMotorJoint.setAngularSpeed(0);
		}

		// System.out.println("Rotation --> "
		// + (stick.getRotation() - bigBlock.getRotation()));
	}

	public static void main(String s[]) throws InterruptedException {
		final PhysicsWorld physicsWorld = new PhysicsWorld();
		PhysRobot physRobot = new ChainRobo();
		physicsWorld.target=1000/500;
		new Thread() {
			public void run() {
				physicsWorld.start();
			}
		}.start();
		physRobot.read();
		Thread.sleep(500);
		physicsWorld.addRobot(physRobot);
	}

	@Override
	public Vector<Action> getAvailableActions() {
		// TODO Auto-generated method stub
		// Vector<Action> vector = new Vector<Action>();
		// double la, ha;
		// la = -2 * Math.PI;
		// ha = 0;
		// double ang = mod(-stick.getRotation() + bigBlock.getRotation() - la,
		// 2 * Math.PI);
		//
		// double mid = Math.PI + (ha - la) / 2;
		//
		// if (ang > 0 && ang < mid)
		// vector.add(rot_cw);
		// if ((ang > 0 && ang < (ha - la)) || (ang > mid && ang < 2 * Math.PI))
		// vector.add(rot_ccw);
		Vector<Action> vector = new Vector<Action>();
		return as;
	}

}
